# -*- coding: utf-8 -*
import serial
import cmath
import time
import datetime

# 打开串口
ser = serial.Serial("/dev/ttyUSB0", 115200)
time_delay = 0.1  # default f = 10 HZ and MAX frequence is 100HZ
dt_ms = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')  # 含微秒的日期时间，来源 比特量化

LOG_FILE = open("Range_sensor_logging.txt", 'a+')
LOG_FILE.write("logginig info write time {} ".format(dt_ms))


def main():
    if ser:
        print("port in {} is opened".format(ser.port))
        LOG_FILE.write("logginig info write time {} \n port in {} is opened".format(
            datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), ser.port))

    else:
        print("port in {}, {} Failed".format(ser.port, ser.baudrate))
        LOG_FILE.write("logginig info write time {} \n port in {} is Failed".format(
            datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), ser.port))
        LOG_FILE.close()
        return -1
    while True:
        # 获得接收缓冲区字符
        count = ser.inWaiting()

        if count != 0:
            # 读取内容并回显
            raw_recv = ser.read(count)
            if raw_recv == None:
                # 必要的软件延时
                time.sleep(time_delay)
            raw_recv = ser.read(count)
            hex_data = raw_recv[9:13]
            dst_int = hex2int(hex_data)
            if dst_int == -1:
                print("ERROR: dst_int == -1, distance get failed")
            print(
                "--------- \n data received: \n {}  \n dst in HEX: {} \n dst in int (mm): {}".format(raw_recv, hex_data,
                                                                                                     dst_int))
            LOG_FILE.write(
                "time: {} \t  dst (mm): {} \n".format(datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'),
                                                      dst_int))

        # 清空接收缓冲区
        ser.flushInput()
        # 必要的软件延时
        time.sleep(time_delay)


def hex2int(hex_ss):
    base = {'0': 0, '1': 1, '2': 2, '3': 3, '4': 4, '5': 5, '6': 6, '7': 7, '8': 8, '9': 9, 'A': 10, 'B': 11, 'C': 12,
            'D': 13, 'E': 14, 'F': 15}
    val = [0, 0, 0, 0]
    dst_result = 0
    if hex_ss != None:
        # string input check
        idx = 3
        for str in hex_ss:
            print("str : {}".format(str))
            hex_val = base.get(str)
            val[idx] = hex_val * pow(16, idx)
            idx = idx - 1
        dst_result = val[0] + val[1] + val[2] + val[3]
        return dst_result

    else:
        return -1


if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        if ser != None:
            ser.close()
            LOG_FILE.close()  # close the func and logging file
